import time

import pygame
from pygame.locals import *
from pygame.tests.base_test import pygame_quit

import Project.Control.CommandGeneratorClass as CGC
import Project.Control.ControlFunctions as Functions
import Project.Control.RobotControl as RobotControl
import Project.Main as Main
import Project.Simulation.Robotics.Client as ClientClass
import Project.Simulation.SimulationInterface as SI
import robo3pi as s


# Initialize connection with the robots
s.init('COM' + str(Main.COM_PORT))

# Initialize Pygame
pygame.init()
screen = pygame.display.set_mode((100, 100))
pygame.display.set_caption('Polulu')
pygame.mouse.set_visible(0)

#Open a socket that will be used to connect to simulation server
client = ClientClass.Client("127.0.0.1")
client.start()
centerList = []



done = False

# Infinite loop for receiving user input
while not done:
    event = pygame.event.poll()
    if not (event.type == pygame.NOEVENT):
        
        if (event.type == KEYUP):
            if (event.key == K_1):
                SI.generateWorkspace()
                SI.generateRobot()
                robotArray, targetPoints = SI.generateTarget()
            elif (event.key == K_2):
                robotArray, sourcePoints = SI.generateSource()
            elif (event.key == K_3):
                addr = (Main.SIMULATION_SERVER_IP,Main.SIMLUATION_SERVER_PORT)
                client.connect(addr)
                client.send_input(Main.FILES_PATH,addr)
            elif (event.key == K_4):
                myGenerator = CGC.CommandGeneratorClass(Main.FILES_PATH + "path.txt" , 1, robotArray, sourcePoints)
                RobotControl.RunAllCommands(myGenerator, robotArray, targetPoints)
            elif (event.key == K_ESCAPE):
                #myGenerator.close()
                client.stop()
                s.close()
                done = True
    
    # Short sleep between commands to verify that user input is slightly delayed if needed
    time.sleep(0.15)
          
# Quit when finished receiving user input
pygame.quit()